#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

// 引脚定义
#define POTENTIOMETER_PIN 33
#define X_PIN 34
#define Y_PIN 35

// 油门引脚
#define SWITCH_PIN 4

#define OLED_SCL 17
#define OLED_SDA 16

const int gear_back = -1;
const int gear_advance = 1;
const int gear_super_advance = 2;

// 电机1引脚定义
const int RPWM_PIN = 18;   // 连接BTS7960 RPWM
const int LPWM_PIN = 19;   // 连接BTS7960 LPWM
const int ENABLE_PIN = 5;  // 连接R_EN和L_EN

// 电机2引脚定义
const int RPWM_PIN2 = 12;  // 连接BTS7960 RPWM
const int LPWM_PIN2 = 13;  // 连接BTS7960 LPWM
const int ENABLE_PIN2 = 2; // 连接R_EN和L_EN

// PWM参数
const int PWM_FREQ = 5000;      // PWM频率(Hz)
const int PWM_RESOLUTION = 8;   // 8位分辨率(0-255)
const int grad = 20;            // 电机加速度

int currentLeftSpeed = 0;
int currentRightSpeed = 0;
int speed = 0;

// 转向参数 - 修改最小转向速度为50
const int minTurnSpeed = 50;    // 最小转向速度
const int maxTurnOffset = 150;  // 最大转向速度差（增加范围）

// 定义电机通道结构体
struct MotorChannels {
  int channel_r;
  int channel_l;
};

// 创建电机通道配置
MotorChannels leftMotor = {0, 1};   // 左侧电机使用通道0和1
MotorChannels rightMotor = {2, 3};  // 右侧电机使用通道2和3

// 摇杆校准参数
int centerX = 1900;        // 中心点X值
int centerY = 1840;        // 中心点Y值
const int deadZone = 300;  // 调整为更合理的死区值

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ========== 函数前置声明 ==========
void setMotorSpeed(int speed, char motor);
void stopMotor(char motor);
int gear_state(float voltage, int gear);
int approachZero(int speed);
void updateMotorSpeeds(int& leftTarget, int& rightTarget, int maxSpeed, int rawX, int rawY);

void setup() {
  delay(1000);
  Serial.begin(115200);
  // 初始化开关
  pinMode(SWITCH_PIN, INPUT_PULLUP);  // 使用内部上拉电阻

  // 初始化I2C
  Wire.begin(OLED_SDA, OLED_SCL);

  // 初始化OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (1);  // 卡死
  }

  // 显示欢迎信息
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 10);
  display.println(F("Joystick Monitor"));
  display.println(F("Initialized..."));
  display.display();
  delay(1000);  // 显示1秒

  // 初始化使能引脚
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, HIGH);  // 启用驱动芯片
  pinMode(ENABLE_PIN2, OUTPUT);
  digitalWrite(ENABLE_PIN2, HIGH);  // 启用驱动芯片

  // 配置PWM通道
  ledcSetup(leftMotor.channel_r, PWM_FREQ, PWM_RESOLUTION);
  ledcSetup(leftMotor.channel_l, PWM_FREQ, PWM_RESOLUTION);
  ledcSetup(rightMotor.channel_r, PWM_FREQ, PWM_RESOLUTION);
  ledcSetup(rightMotor.channel_l, PWM_FREQ, PWM_RESOLUTION);

  // 绑定PWM通道到GPIO
  ledcAttachPin(RPWM_PIN, leftMotor.channel_r);
  ledcAttachPin(LPWM_PIN, leftMotor.channel_l);
  ledcAttachPin(RPWM_PIN2, rightMotor.channel_r);
  ledcAttachPin(LPWM_PIN2, rightMotor.channel_l);

  // 初始停止电机
  stopMotor('l');
  stopMotor('r');
}

void loop() {
  // 读取传感器数据
  int rawX = analogRead(X_PIN);
  int rawY = analogRead(Y_PIN);
  int sensorValue = analogRead(POTENTIOMETER_PIN);
  int gear = 0;
  // 计算电压值 (0-3.3V)
  float voltage = sensorValue * (3.3 / 4095.0);
  // 开关定义
  int switchState = digitalRead(SWITCH_PIN);

  // 摇杆值中心校准
  int centeredX = rawX - centerX;
  int centeredY = rawY - centerY;

  // 防抖
  if (abs(centeredX) < deadZone) centeredX = 0;
  if (abs(centeredY) < deadZone) centeredY = 0;
  gear = gear_state(voltage, gear);

  // 将xy映射到-255--255
  int mappedY;
  if (centeredY <= 0) {
    mappedY = map(centeredY, -2048, 0, -255, 0); // 使用ADC范围(0-4095)
  } else {
    mappedY = map(centeredY, 0, 2048, 0, 255);
  }
  
  int mappedX;
  if (centeredX <= 0) {
    mappedX = map(centeredX, -2048, 0, -255, 0);
  } else {
    mappedX = map(centeredX, 0, 2048, 0, 255);
  }

  // 计算目标速度（基于摇杆和挡位）
  int leftTarget = 0;
  int rightTarget = 0;
  int maxSpeed = 0;
  
  if (switchState == HIGH) { // 油门开启状态
    // 根据挡位设置最大速度
    switch (gear) {
      case gear_back: maxSpeed = 150; break;          // 后退限速
      case gear_advance: maxSpeed = 200; break;       // 普通前进
      case gear_super_advance: maxSpeed = 255; break; // 全速前进
      default: maxSpeed = 0;
    }

    // 基础速度计算（Y轴控制前进/后退）
    int baseSpeed = 0;
    if (gear == gear_advance || gear == gear_super_advance) {
      if (mappedY > 0) {
        baseSpeed = map(mappedY, 0, 255, 0, maxSpeed);
      }
    } else if (gear == gear_back) {
      if (mappedY < 0) {
        baseSpeed = map(mappedY, -255, 0, -maxSpeed, 0);
      }
    }

    // 设置左右电机基础速度
    leftTarget = baseSpeed;
    rightTarget = baseSpeed;

    // 处理转向（X轴控制）
    if (abs(mappedX) > 20) { // 转向死区，避免微小偏移
      // 计算转向偏移量（基于X绝对值）
      int turnOffset = map(abs(mappedX), 0, 255, 0, maxTurnOffset);
      
      // 应用转向偏移（差速转向）
      if (mappedX > 0) { // 右转
        // 确保右侧轮子速度不低于50
        if (rightTarget > 0) {
          rightTarget = max(minTurnSpeed, rightTarget - turnOffset);
        } else if (rightTarget < 0) {
          rightTarget = min(-minTurnSpeed, rightTarget + turnOffset);
        }
      } else { // 左转
        // 确保左侧轮子速度不低于50
        if (leftTarget > 0) {
          leftTarget = max(minTurnSpeed, leftTarget - turnOffset);
        } else if (leftTarget < 0) {
          leftTarget = min(-minTurnSpeed, leftTarget + turnOffset);
        }
      }
    }
  } else {
    // 油门关闭时平滑停止
    leftTarget = 0;
    rightTarget = 0;
    currentLeftSpeed = approachZero(currentLeftSpeed);
    currentRightSpeed = approachZero(currentRightSpeed);
  }

  // 渐进调整左电机速度
  if (currentLeftSpeed < leftTarget)
    currentLeftSpeed = min(currentLeftSpeed + grad, leftTarget);
  else if (currentLeftSpeed > leftTarget)
    currentLeftSpeed = max(currentLeftSpeed - grad, leftTarget);
    
  // 渐进调整右电机速度
  if (currentRightSpeed < rightTarget)
    currentRightSpeed = min(currentRightSpeed + grad, rightTarget);
  else if (currentRightSpeed > rightTarget)
    currentRightSpeed = max(currentRightSpeed - grad, rightTarget);

  // 更新电机速度
  setMotorSpeed(currentLeftSpeed, 'l');
  setMotorSpeed(currentRightSpeed, 'r');
  
  // 更新速度显示值
  speed = (abs(currentLeftSpeed) + abs(currentRightSpeed)) / 2;

  // 清除并准备显示
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  
  // 显示油门状态
  display.print(F("Throttle: "));
  display.println(switchState == LOW ? "ON" : "OFF");
  
  // 显示挡位状态
  display.print(F("Gear: "));
  display.print(gear);
  switch (gear) {
    case gear_back: display.println(F(" Back")); break;
    case gear_advance: display.println(F(" Advance")); break;
    case gear_super_advance: display.println(F(" SUPER ADVANCE")); break;
    default: display.println(F(" Unknown")); break;
  }
  
  // 显示方向状态
  if (gear == gear_advance || gear == gear_super_advance) {
    if (mappedY > 0) {
      display.print(F("Direction: FORWARD "));
      display.print(map(mappedY, 0, 255, 0, 100));
      display.println(F("%"));
    } else {
      display.println(F("Direction: STOP"));
    }
  } else if (gear == gear_back) {
    if (mappedY < 0) {
      display.print(F("Direction: BACKWARD "));
      display.print(map(abs(mappedY), 0, 255, 0, 100));
      display.println(F("%"));
    } else {
      display.println(F("Direction: STOP"));
    }
  } else {
    display.println(F("Direction: STOP"));
  }
  
  display.print(F("Speed: "));
  display.print(speed);
  display.print(F(" (L:"));
  display.print(currentLeftSpeed);
  display.print(F(" R:"));
  display.print(currentRightSpeed);
  display.println(F(")"));
  
  // 显示摇杆原始值
  display.print(F("rawX: "));
  display.print(rawX);
  display.print(F(" ("));
  display.print(centeredX);
  display.print(F(")"));
  
  display.print(F(" rawY: "));
  display.print(rawY);
  display.print(F(" ("));
  display.print(centeredY);
  display.println(F(")"));
  
  // 显示映射值
  display.print(F("mappedX: "));
  display.print(mappedX);
  display.print(F(" mappedY: "));
  display.println(mappedY);
  
  // 显示转向状态
  if (mappedX > 30) {
    display.println(F("Turning RIGHT"));
    display.print(F("Turn Offset: "));
    display.print(map(abs(mappedX), 0, 255, 0, maxTurnOffset));
  } else if (mappedX < -30) {
    display.println(F("Turning LEFT"));
    display.print(F("Turn Offset: "));
    display.print(map(abs(mappedX), 0, 255, 0, maxTurnOffset));
  } else {
    display.println(F("Going STRAIGHT"));
  }

  // 更新显示
  display.display();

  delay(100);  // 刷新率100ms
}

void setMotorSpeed(int speed, char motor) {
  MotorChannels channels;
  
  // 选择电机
  if (motor == 'l') {
    channels = leftMotor;
  } else if (motor == 'r') {
    channels = rightMotor;
  } else {
    return;  // 无效电机标识
  }

  if (speed > 0) {
    // 正转
    ledcWrite(channels.channel_r, speed);
    ledcWrite(channels.channel_l, 0);
  } else if (speed < 0) {
    // 反转
    ledcWrite(channels.channel_r, 0);
    ledcWrite(channels.channel_l, -speed);
  } else {
    // 停止
    ledcWrite(channels.channel_r, 0);
    ledcWrite(channels.channel_l, 0);
  }
}

// 停止特定电机
void stopMotor(char motor) {
  setMotorSpeed(0, motor);
}

int gear_state(float voltage, int gear) {
  // 挡位控制
  if (voltage < 1.1) 
    gear = gear_back;
  else if (voltage >= 1.1 && voltage < 2.2) 
    gear = gear_advance;
  else if (voltage >= 2.2 && voltage <= 3.4) 
    gear = gear_super_advance;
  
  return gear;
}

int approachZero(int speed) {
  if (speed > 0) 
    return max(0, speed - grad);
  if (speed < 0) 
    return min(0, speed + grad);
  return 0;
}